#include "mindvision_camera/mv_camera_node.hpp"

// ROS 相关头文件
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"

// C++ 系统头文件
#include <string>
#include <vector>

namespace mindvision_camera {

// -------------------------- 构造/析构函数实现 --------------------------
MVCameraNode::MVCameraNode(const rclcpp::NodeOptions &options) 
  : Node("mindvision_camera_node", options), camera_sdk_(this->get_logger())
{
  RCLCPP_INFO(this->get_logger(), "Starting MVCameraNode...");

  // 1. 初始化相机 SDK
  if (!camera_sdk_.init()) {
    RCLCPP_FATAL(this->get_logger(), "Camera SDK initialization failed! Node will exit.");
    rclcpp::shutdown();
    return;
  }

  // 2. 声明所有 ROS 参数（依赖 SDK 获取默认值和范围）
  declareAllParameters();

  // 3. 创建图像发布者（处理 QoS 配置）
  createImagePublisher();

  // 4. 加载相机内参（从配置文件或默认路径）
  loadCameraInfo();

  // 5. 绑定动态参数回调（处理参数修改）
  bindParametersCallback();

  // 6. 启动图像捕获线程（循环发布图像）
  startCaptureThread();

  RCLCPP_INFO(this->get_logger(), "MVCameraNode initialized successfully!");
}

MVCameraNode::~MVCameraNode()
{
  // 等待捕获线程退出
  if (capture_thread_.joinable()) {
    capture_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Capture thread joined successfully.");
  }

  RCLCPP_INFO(this->get_logger(), "MVCameraNode destroyed.");
}

// -------------------------- 核心初始化函数实现 --------------------------
void MVCameraNode::declareAllParameters()
{
  // 1. 创建参数描述符（用于设置参数说明、范围等）
  rcl_interfaces::msg::ParameterDescriptor param_desc;
  param_desc.integer_range.resize(1);  // 整数参数的范围配置
  param_desc.integer_range[0].step = 1;  // 步长为 1（仅整数）

  // 2. 从 SDK 获取相机能力（参数范围、分辨率等）
  auto camera_cap = camera_sdk_.getCapability();
  auto exposure_line_time = camera_sdk_.getExposureLineTime();  // 曝光行时间（计算曝光范围用）

  // -------------------------- 2.1 曝光时间参数 --------------------------
  param_desc.description = "Exposure time in microseconds (must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sExposeDesc.uiExposeTimeMin * exposure_line_time;
  param_desc.integer_range[0].to_value = camera_cap.sExposeDesc.uiExposeTimeMax * exposure_line_time;
  double exposure_time = this->declare_parameter("exposure_time", 5000.0, param_desc);
  // 声明后同步到相机硬件
  if (!camera_sdk_.setExposureTime(exposure_time)) {
    RCLCPP_WARN(this->get_logger(), "Failed to set initial exposure time: %.1f us", exposure_time);
  } else {
    RCLCPP_INFO(this->get_logger(), "Initial exposure time set to %.1f us", exposure_time);
  }

  // -------------------------- 2.2 模拟增益参数 --------------------------
  param_desc.description = "Analog gain (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sExposeDesc.uiAnalogGainMin;
  param_desc.integer_range[0].to_value = camera_cap.sExposeDesc.uiAnalogGainMax;
  int analog_gain = this->declare_parameter("analog_gain", camera_sdk_.getCurrentAnalogGain(), param_desc);
  // 声明后同步到相机硬件
  if (!camera_sdk_.setAnalogGain(analog_gain)) {
    RCLCPP_WARN(this->get_logger(), "Failed to set initial analog gain: %d", analog_gain);
  } else {
    RCLCPP_INFO(this->get_logger(), "Initial analog gain set to %d", analog_gain);
  }

  // -------------------------- 2.3 RGB 增益参数 --------------------------
  // 从 SDK 获取当前 RGB 增益（作为默认值）
  camera_sdk_.getCurrentRgbGain(r_gain_, g_gain_, b_gain_);
  // R 通道增益
  param_desc.description = "Red channel gain (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sRgbGainRange.iRGainMin;
  param_desc.integer_range[0].to_value = camera_cap.sRgbGainRange.iRGainMax;
  r_gain_ = this->declare_parameter("rgb_gain.r", r_gain_, param_desc);
  // G 通道增益
  param_desc.description = "Green channel gain (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sRgbGainRange.iGGainMin;
  param_desc.integer_range[0].to_value = camera_cap.sRgbGainRange.iGGainMax;
  g_gain_ = this->declare_parameter("rgb_gain.g", g_gain_, param_desc);
  // B 通道增益
  param_desc.description = "Blue channel gain (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sRgbGainRange.iBGainMin;
  param_desc.integer_range[0].to_value = camera_cap.sRgbGainRange.iBGainMax;
  b_gain_ = this->declare_parameter("rgb_gain.b", b_gain_, param_desc);
  // 声明后同步到相机硬件（RGB 增益需三通道同时设置）
  if (!camera_sdk_.setRgbGain(r_gain_, g_gain_, b_gain_)) {
    RCLCPP_WARN(this->get_logger(), "Failed to set initial RGB gain: R=%d, G=%d, B=%d", r_gain_, g_gain_, b_gain_);
  } else {
    RCLCPP_INFO(this->get_logger(), "Initial RGB gain set to R=%d, G=%d, B=%d", r_gain_, g_gain_, b_gain_);
  }

  // -------------------------- 2.4 饱和度参数 --------------------------
  param_desc.description = "Saturation (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sSaturationRange.iMin;
  param_desc.integer_range[0].to_value = camera_cap.sSaturationRange.iMax;
  int saturation = this->declare_parameter("saturation", camera_sdk_.getCurrentSaturation(), param_desc);
  // 声明后同步到相机硬件
  if (!camera_sdk_.setSaturation(saturation)) {
    RCLCPP_WARN(this->get_logger(), "Failed to set initial saturation: %d", saturation);
  } else {
    RCLCPP_INFO(this->get_logger(), "Initial saturation set to %d", saturation);
  }

  // -------------------------- 2.5 伽马值参数 --------------------------
  param_desc.description = "Gamma value (integer, must be within camera's support range)";
  param_desc.integer_range[0].from_value = camera_cap.sGammaRange.iMin;
  param_desc.integer_range[0].to_value = camera_cap.sGammaRange.iMax;
  int gamma = this->declare_parameter("gamma", camera_sdk_.getCurrentGamma(), param_desc);
  // 声明后同步到相机硬件
  if (!camera_sdk_.setGamma(gamma)) {
    RCLCPP_WARN(this->get_logger(), "Failed to set initial gamma: %d", gamma);
  } else {
    RCLCPP_INFO(this->get_logger(), "Initial gamma set to %d", gamma);
  }

  // -------------------------- 2.6 图像翻转参数 --------------------------
  flip_image_ = this->declare_parameter("flip_image", false);
  RCLCPP_INFO(this->get_logger(), "Initial image flip set to %s", flip_image_ ? "true" : "false");

  // -------------------------- 2.7 VID/PID 参数（USB 重置用） --------------------------
  std::string vid_pid = this->declare_parameter("vid_pid", "0x1BDA:0x0011");
  camera_sdk_.setVidPid(vid_pid);  // 同步到 SDK

  // -------------------------- 2.8 QoS 配置参数 --------------------------
  use_sensor_data_qos_ = this->declare_parameter("use_sensor_data_qos", false);
  RCLCPP_INFO(this->get_logger(), "Use sensor_data QoS set to %s", use_sensor_data_qos_ ? "true" : "false");

  // -------------------------- 2.9 相机名称与内参 URL 参数 --------------------------
  camera_name_ = this->declare_parameter("camera_name", "mv_camera");
  // 相机内参 URL 在 loadCameraInfo() 中声明，此处不重复
}

void MVCameraNode::createImagePublisher()
{
  // 选择 QoS 配置（sensor_data 适合高频图像，default 兼容 rqt_image_view）
  auto qos_profile = use_sensor_data_qos_ ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;

  // 创建图像+内参联合发布者（话题名：image_raw，包含 image_raw 和 image_raw/camera_info）
  camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos_profile);

  //创建第二套相机内参发布器
  scope_camera_info_pub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("scope_camera_info", use_sensor_data_qos_ ? rclcpp::SensorDataQoS() : rclcpp::QoS(rclcpp::KeepLast(1)));

  // 预分配图像数据内存（避免频繁扩容，提升性能）
  auto camera_cap = camera_sdk_.getCapability();
  size_t max_data_size = camera_cap.sResolutionRange.iWidthMax * camera_cap.sResolutionRange.iHeightMax * 3;
  image_msg_.data.reserve(max_data_size);

  // 初始化图像消息固定字段
  image_msg_.encoding = "rgb8";  // 与 SDK 输出格式（CAMERA_MEDIA_TYPE_RGB8）一致
  image_msg_.header.frame_id = "camera_optical_frame";  // 相机光学坐标系（需根据实际调整）
}

void MVCameraNode::loadCameraInfo()
{
  // 1. 声明相机内参 URL 参数（默认路径：包内 config 目录下的 camera_info.yaml）
  std::string default_camera_info_url = "package://mindvision_camera/config/camera_info.yaml";
  std::string camera_info_url = this->declare_parameter("camera_info_url", default_camera_info_url);

  // 2. 初始化内参管理器
  camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);

  // 3. 加载内参（验证 URL 有效性）
  if (camera_info_manager_->validateURL(camera_info_url)) {
    if (camera_info_manager_->loadCameraInfo(camera_info_url)) {
      camera_info_msg_ = camera_info_manager_->getCameraInfo();
      RCLCPP_INFO(this->get_logger(), "Loaded camera info from URL: %s", camera_info_url.c_str());
    } 
  } else {
    RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s", camera_info_url.c_str());
    // 若内参无效，使用默认空内参（避免发布失败）
    camera_info_msg_ = sensor_msgs::msg::CameraInfo();
  }

  // 初始化内参消息的 frame_id（与图像消息一致）
  camera_info_msg_.header.frame_id = image_msg_.header.frame_id;

  //第二套scope相机参数
  std::string default_scope_camera_info_url = "package://mindvision_camera/config/scope_camera_info.yaml";
  std::string scope_camera_info_url = this->declare_parameter("scope_camera_info_url", default_scope_camera_info_url);

  scope_camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_ + "_2"); // 内参管理器名称区分
  if (scope_camera_info_manager_->validateURL(scope_camera_info_url)) {
      if (scope_camera_info_manager_->loadCameraInfo(scope_camera_info_url)) {
        scope_camera_info_msg_ = scope_camera_info_manager_->getCameraInfo();
        RCLCPP_INFO(this->get_logger(), "Loaded scope camera info from: %s", scope_camera_info_url.c_str());
      }
    } else {
        RCLCPP_WARN(this->get_logger(), "Invalid scope camera info URL: %s", scope_camera_info_url.c_str());
        scope_camera_info_msg_ = sensor_msgs::msg::CameraInfo();
    }
  scope_camera_info_msg_.header.frame_id = image_msg_.header.frame_id;
}

void MVCameraNode::bindParametersCallback()
{
  // 绑定动态参数回调函数（参数修改时自动触发 parametersCallback）
  params_callback_handle_ = this->add_on_set_parameters_callback(
    std::bind(&MVCameraNode::parametersCallback, this, std::placeholders::_1)
  );
}

void MVCameraNode::startCaptureThread()
{
  // 启动图像捕获线程（使用 lambda 表达式作为线程函数）
  capture_thread_ = std::thread{[this]() -> void {
  RCLCPP_INFO(this->get_logger(), "Capture thread started. Publishing images...");

    while (rclcpp::ok()) {
      tSdkFrameHead frame_info;  // SDK 图像帧头（分辨率、帧率等）
      uint8_t *raw_buffer = nullptr;  // SDK 原始图像缓冲区
      try
      {
        // 1. 从 SDK 获取图像缓冲区（超时 1000ms）
      if (!camera_sdk_.getImageBuffer(frame_info, raw_buffer, 1000)) {
        camera_sdk_.releaseImageBuffer();
         throw std::runtime_error("Failed to get image buffer!");
        
      }
      // 2. 处理图像（ISP 转换为 RGB8 + 翻转，结果存入 image_msg_.data）
      image_msg_.data.resize(frame_info.iWidth * frame_info.iHeight * 3);  // 分配实际数据大小
      camera_sdk_.processImage(image_msg_.data.data(), flip_image_);

      // 3. 更新图像消息动态字段（时间戳、分辨率、步长）
      auto current_time = this->now();
      image_msg_.header.stamp = current_time;
      image_msg_.height = frame_info.iHeight;
      image_msg_.width = frame_info.iWidth;
      image_msg_.step = frame_info.iWidth * 3;  // RGB8 格式：每个像素 3 字节

      // 4. 更新相机内参消息（时间戳、分辨率与图像一致）
      camera_info_msg_.header.stamp = current_time;
      camera_info_msg_.height = frame_info.iHeight;
      camera_info_msg_.width = frame_info.iWidth;

      // 第二套scope相机内参
      scope_camera_info_msg_.header.stamp = current_time;
      scope_camera_info_msg_.height = frame_info.iHeight;
      scope_camera_info_msg_.width = frame_info.iWidth;

      // 5. 发布图像和两套内参消息
      camera_pub_.publish(image_msg_, camera_info_msg_);
      scope_camera_info_pub_->publish(scope_camera_info_msg_);

      // 6. 释放 SDK 图像缓冲区（必须调用，否则下次获取会阻塞）
      camera_sdk_.releaseImageBuffer();
      }
      catch(const std::exception& e)
      {
        RCLCPP_ERROR(this->get_logger(), "Error processing image: %s. Triggering restart...", e.what());
        restartCamera();
      }
    }
  }};
}
void MVCameraNode::restartCamera()
{
   RCLCPP_WARN(this->get_logger(), "Starting camera restart (max 3 retries)...");

  // 1. 强制释放SDK资源（避免残留）
  if (camera_sdk_.isCameraInitialized()) {  
    camera_sdk_.unInit();
    std::this_thread::sleep_for(std::chrono::milliseconds(500)); 
    RCLCPP_INFO(this->get_logger(), "Camera SDK uninitialized.");
  }

  // 2. 重试初始化
  while (rclcpp::ok()) {
    // USB重置后必须等硬件恢复（否则初始化必败）
    camera_sdk_.resetUsb();
    std::this_thread::sleep_for(std::chrono::seconds(1));  // 给USB枚举/相机上电留时间

    // 尝试初始化
    if (camera_sdk_.init()) {
      RCLCPP_INFO(this->get_logger(), "Camera restarted successfully !");
      syncParametersToCamera();
      break;
    }

    RCLCPP_ERROR(this->get_logger(), "Camera restart failed. Retrying...");
    // 重试前再次释放资源（防止前一次初始化残留）
    camera_sdk_.unInit();
    std::this_thread::sleep_for(std::chrono::seconds(1));
  }
}
void MVCameraNode::syncParametersToCamera()
{
  // 重新设置所有关键参数（曝光、增益等）
  camera_sdk_.setExposureTime(this->get_parameter("exposure_time").as_double());
  camera_sdk_.setAnalogGain(this->get_parameter("analog_gain").as_int());
  camera_sdk_.setRgbGain(r_gain_, g_gain_, b_gain_);
  camera_sdk_.setSaturation(this->get_parameter("saturation").as_int());
  camera_sdk_.setGamma(this->get_parameter("gamma").as_int());
}
// -------------------------- 动态参数回调函数实现 --------------------------
rcl_interfaces::msg::SetParametersResult MVCameraNode::parametersCallback(
  const std::vector<rclcpp::Parameter> &parameters)
{
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;  // 默认成功
  result.reason = "All parameters set successfully.";

  // 遍历所有被修改的参数
  for (const auto &param : parameters) {
    const std::string &param_name = param.get_name();

    // -------------------------- 曝光时间参数 --------------------------
    if (param_name == "exposure_time") {
      double new_exposure = param.as_double();
      if (!camera_sdk_.setExposureTime(new_exposure)) {
        result.successful = false;
        result.reason = "Failed to set exposure time: " + std::to_string(new_exposure) + " us";
        break;
      }
      RCLCPP_INFO(this->get_logger(), "Exposure time updated to %.1f us", new_exposure);
    }

    // -------------------------- 模拟增益参数 --------------------------
    else if (param_name == "analog_gain") {
      int new_gain = param.as_int();
      if (!camera_sdk_.setAnalogGain(new_gain)) {
        result.successful = false;
        result.reason = "Failed to set analog gain: " + std::to_string(new_gain);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "Analog gain updated to %d", new_gain);
    }

    // -------------------------- RGB 增益参数（R 通道） --------------------------
    else if (param_name == "rgb_gain.r") {
      r_gain_ = param.as_int();
      if (!camera_sdk_.setRgbGain(r_gain_, g_gain_, b_gain_)) {
        result.successful = false;
        result.reason = "Failed to update RGB gain (R): R=" + std::to_string(r_gain_) + ", G=" + std::to_string(g_gain_) + ", B=" + std::to_string(b_gain_);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "RGB gain updated: R=%d, G=%d, B=%d", r_gain_, g_gain_, b_gain_);
    }

    // -------------------------- RGB 增益参数（G 通道） --------------------------
    else if (param_name == "rgb_gain.g") {
      g_gain_ = param.as_int();
      if (!camera_sdk_.setRgbGain(r_gain_, g_gain_, b_gain_)) {
        result.successful = false;
        result.reason = "Failed to update RGB gain (G): R=" + std::to_string(r_gain_) + ", G=" + std::to_string(g_gain_) + ", B=" + std::to_string(b_gain_);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "RGB gain updated: R=%d, G=%d, B=%d", r_gain_, g_gain_, b_gain_);
    }

    // -------------------------- RGB 增益参数（B 通道） --------------------------
    else if (param_name == "rgb_gain.b") {
      b_gain_ = param.as_int();
      if (!camera_sdk_.setRgbGain(r_gain_, g_gain_, b_gain_)) {
        result.successful = false;
        result.reason = "Failed to update RGB gain (B): R=" + std::to_string(r_gain_) + ", G=" + std::to_string(g_gain_) + ", B=" + std::to_string(b_gain_);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "RGB gain updated: R=%d, G=%d, B=%d", r_gain_, g_gain_, b_gain_);
    }

    // -------------------------- 饱和度参数 --------------------------
    else if (param_name == "saturation") {
      int new_saturation = param.as_int();
      if (!camera_sdk_.setSaturation(new_saturation)) {
        result.successful = false;
        result.reason = "Failed to set saturation: " + std::to_string(new_saturation);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "Saturation updated to %d", new_saturation);
    }

    // -------------------------- 伽马值参数 --------------------------
    else if (param_name == "gamma") {
      int new_gamma = param.as_int();
      if (!camera_sdk_.setGamma(new_gamma)) {
        result.successful = false;
        result.reason = "Failed to set gamma: " + std::to_string(new_gamma);
        break;
      }
      RCLCPP_INFO(this->get_logger(), "Gamma updated to %d", new_gamma);
    }

    // -------------------------- 图像翻转参数 --------------------------
    else if (param_name == "flip_image") {
      flip_image_ = param.as_bool();
      RCLCPP_INFO(this->get_logger(), "Image flip updated to %s", flip_image_ ? "true" : "false");
    }

    // -------------------------- VID/PID 参数 --------------------------
    else if (param_name == "vid_pid") {
      std::string new_vid_pid = param.as_string();
      camera_sdk_.setVidPid(new_vid_pid);
      RCLCPP_INFO(this->get_logger(), "VID/PID updated to %s", new_vid_pid.c_str());
    }

    // -------------------------- 未知参数 --------------------------
    else {
      result.successful = false;
      result.reason = "Unknown parameter: " + param_name;
      break;
    }
  }

  return result;
}

}  // namespace mindvision_camera

// -------------------------- 注册 ROS 组件 --------------------------
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(mindvision_camera::MVCameraNode)